#define ROVIO_CAM "http://143.215.111.166/Jpeg/CamImg[001].jpg"

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv/cxcore.h>
#include "httpInterface.h"
using namespace cv;

typedef struct {
  int hueL,hueH,satL,satH,valL,valH;
} ColorParam;

KeyPoint FindLargestBlob(const vector<KeyPoint>& keyPoints);
void ColorThreshold(const Mat& src, Mat& dst, const ColorParam& param);
int FindPoints(const Mat& camImage, const SimpleBlobDetector& blobDetector, Point& rovioL, Point& rovioR, Point& fruit);

int main ( int argc, char **argv )
{
	VideoCapture camera(0); // video source for webcam
	camera.set(CV_CAP_PROP_FRAME_WIDTH,640);
	camera.set(CV_CAP_PROP_FRAME_HEIGHT,480);
	// create matrices to hold image
	Mat camImage;		// raw image from webcam
	Mat final;			// Final display image
	
	// Initialize blob detector
	SimpleBlobDetector::Params params;
	params.minThreshold = 40;
	params.maxThreshold = 60;
	params.thresholdStep = 5;
	params.minArea = 500; 
	params.minConvexity = 0;
	params.minInertiaRatio = 0;
	params.maxArea = 60000;
	params.maxConvexity = 255;
	params.filterByColor = false;
	params.filterByCircularity = false;
	SimpleBlobDetector blobDetector( params );
	blobDetector.create("SimpleBlob");
	
	// Theshold Settings
	int blurSize = 1;
  
	// make tune bar
	ColorParam testColor;
	testColor.hueL = 170, testColor.hueH = 180;
	testColor.satL = 120,testColor.satH = 255;
	testColor.valL = 120,testColor.valH = 255;
	cvNamedWindow("Color Tune",CV_WINDOW_NORMAL);
	cvCreateTrackbar( "Hue UpperT","Color Tune", &testColor.hueH, 255, 0 );
	cvCreateTrackbar ("Hue LowerT","Color Tune", &testColor.hueL,255, 0);
	cvCreateTrackbar( "Sat UpperT","Color Tune", &testColor.satH, 255, 0 );
	cvCreateTrackbar( "Sat LowerT","Color Tune", &testColor.satL, 255, 0 );
	cvCreateTrackbar( "Val UpperT","Color Tune", &testColor.valH, 255, 0 );
	cvCreateTrackbar( "Val LowerT","Color Tune", &testColor.valL, 255, 0 );
	cvCreateTrackbar ("BlurSize","Color Tune", &blurSize,15, 0);

	// check blurSize bound
	if(blurSize == 0)
	  blurSize = 1; //reset blurSize

	Mat rovImage;
	for(;;)
	{
	  Point rovioL, rovioR, fruit;
	  camera >> camImage; // Get new image
	  camImage.copyTo(final);
	  
	  if( FindPoints(camImage, blobDetector, rovioL, rovioR, fruit) ) {
	    circle(final, rovioL, 20, cvScalar(255,0,0), 10);
	    circle(final, rovioR, 20, cvScalar(255,0,0), 10);
	    circle(final, fruit, 20, cvScalar(255,0,0), 10);
	  }

	  // show images
	  imshow("Webcam Orignal", camImage);
	  imshow("Final",final);

	  //	  http_fetch(ROVIO_CAM, "img.jpg");
	  //	  rovImage = imread("img.jpg", 1);
	  //	  imshow("Cam", rovImage);

        if( cvWaitKey(10) >= 0 )
            break;
	}
	cvDestroyAllWindows();
}

int FindPoints(const Mat& camImage, const SimpleBlobDetector& blobDetector, Point& rovioL, Point& rovioR, Point& fruit)
{
  ColorParam rovioRed,rovioGreen,fruitApple;
  rovioGreen.hueL = 60, rovioGreen.hueH = 80;
  rovioGreen.satL = 100,rovioGreen.satH = 255;
  rovioGreen.valL = 100,rovioGreen.valH = 255;
  rovioRed.hueL = 170, rovioRed.hueH = 180;
  rovioRed.satL = 120,rovioRed.satH = 255;
  rovioRed.valL = 120,rovioRed.valH = 255;
  fruitApple.hueL = 100, fruitApple.hueH = 100;
  fruitApple.satL = 100,fruitApple.satH = 255;
  fruitApple.valL = 100,fruitApple.valH = 255;

  Mat blurImage, hsvImage;
  vector<KeyPoint> keyPoints;
  KeyPoint largestBlob1,largestBlob2;

  blur(camImage, blurImage, Size(5,5)); // blur image for better thresholding
	  
  // Find rovio left side
  ColorThreshold(blurImage, hsvImage, rovioGreen);
  blobDetector.detect(hsvImage, keyPoints);
  if(keyPoints.size() == 0)
    return 0;
  largestBlob1 = FindLargestBlob(keyPoints);
  rovioR = largestBlob1.pt;

  // Find rovio right side and fruit
  ColorThreshold(blurImage, hsvImage, rovioRed);
  blobDetector.detect(hsvImage, keyPoints);
  if(keyPoints.size() == 0)
    return 0;

  largestBlob2.size= 0;
  largestBlob1 = keyPoints[0];
  for(size_t i=1; i<keyPoints.size(); i++)
    {
      if(keyPoints[i].size > largestBlob1.size)
	largestBlob1 = keyPoints[i];
      else if (keyPoints[i].size > largestBlob2.size)
	largestBlob2 = keyPoints[i];
    }
	  
  // Figure out which blob is the right side of Rovio
  float dist1, dist2;
  Point2f blob1 = largestBlob1.pt;
  Point2f blob2 = largestBlob2.pt;
  dist1 = sqrt( pow( (blob1.x - rovioR.x),2.0f ) + pow( (blob1.y - rovioR.y),2.0f ) );
  dist2 = sqrt( pow( (blob2.x - rovioR.x),2.0f ) + pow( (blob2.y - rovioR.y),2.0f ) );
  if(dist1 < dist2) {
    rovioL = largestBlob1.pt;
    fruit = largestBlob2.pt;
  }
  else {
    rovioL = largestBlob2.pt;
    fruit = largestBlob1.pt;
  }
  return 1;
}

KeyPoint FindLargestBlob(const vector<KeyPoint>& keyPoints)
{
  KeyPoint largestBlob;
  largestBlob.size = 0;
  for(size_t i=0; i<keyPoints.size(); i++){
    if(keyPoints[i].size > largestBlob.size)
      largestBlob = keyPoints[i];
  }

  return largestBlob;
}

void ColorThreshold(const Mat& src, Mat& dst, const ColorParam& param)
{
  // slices matrcies that hold H,S and V
  vector<Mat> slices;
  Mat hue;			// hue channel
  Mat hue1;			// Hue upper bound
  Mat hue2;			// Hue lower bound
  Mat hue3;			// hue color filtering
  Mat sat;			// Sat channel
  Mat sat1;			// Sat upper bound
  Mat sat2;			// sat lower bound
  Mat sat3;			// sat color filtering
  Mat val;			// Val channel
  Mat val1;			// Val upper bound
  Mat val2;			// Val lower bound
  Mat val3;			// Val color filtering
  
  // Process Image
  cvtColor (src, dst, CV_BGR2HSV);  // convert raw image to hsv

  split(dst,slices); // split image to H,S and V images
  slices[0].copyTo(hue); // get the hue channel
  slices[1].copyTo(sat);
  slices[2].copyTo(val);

  // apply thresshold for hue channel
  threshold (hue,hue1,param.hueL,255, CV_THRESH_BINARY); // get lower bound
  threshold (hue,hue2,param.hueH,255, CV_THRESH_BINARY_INV); // get upper bound
  hue3 = hue1 & hue2; // multiply to get color range

  // apply thresshold for Sat channel
  threshold (sat,sat1,param.satL,255, CV_THRESH_BINARY); // get lower bound
  threshold (sat, sat2,param.satH,255, CV_THRESH_BINARY_INV); // get upper bound
  sat3 = sat1 & sat2; // multiply 2 matrix to get the sat range

  // apply thresshold for Val channel
  threshold (val,val1,param.valL,255, CV_THRESH_BINARY); // get lower bound
  threshold (val, val2,param.valH,255, CV_THRESH_BINARY_INV); // get upper bound
  val3 = val1 & val2; // multiply 2 matrix to get the val range
		
  dst = hue3 & sat3 & val3; // Merge arrays for final thresholded image
}
